#include <cmath>

#include "classify.h"

namespace perception
{
    namespace algorithm
    {

        Classify::Classify()
        {
            boundingbox_ = std::make_shared<BoundingBox>();
            // heading_ = std::make_shared<Heading>();
            lshape_ = std::make_shared<LShape>();
            lshape2_ = std::make_shared<LShape2>();
        }

        void Classify::Compute(std::vector<LidarObject> &lidar_objects)
        {
            std::vector<int> delete_objs;
            const size_t objects_size = lidar_objects.size();
            if (lidar_objects.empty())
                return;
            for (size_t i = 0; i < objects_size; i++)
            {
                LidarObject &object = lidar_objects[i];
                object.id = id;
                // 计算航向
                auto tmp_cells = object.cluster_points;
                // boundingbox_->Compute(object);
                object.heading = lshape2_->Compute(object.cluster_points);
                lshape2_->CoordinateTransformation(object.cluster_points, object.heading);
                boundingbox_->ComputeLWH(object);
                auto tmp_x = object.x;
                auto tmp_y = object.y;
                object.x = tmp_x * std::cos(-object.heading) + tmp_y * std::sin(-object.heading);
                object.y = tmp_y * std::cos(-object.heading) - tmp_x * std::sin(-object.heading);
                // std::cout << " points1 size:" << object.cluster_points.points.size() << std::endl;
                object.cluster_points = tmp_cells;
                // 重新计算角点
                // boundingbox_->ComputeCornerPoints(object);

                // 20以下为车
                GetType(object);
                id++;
                if (id > 255)
                    id = 0;
                if (object.length < 0 || object.width < 0)
                {
                    std::cout << "i:" << i << " object.length:" << object.length << std::endl;
                    delete_objs.emplace_back(i);
                }
            }
        } // namespace perception
        void Classify::Compute(LidarObject &lidar_object)
        {
            LidarObject &object = lidar_object;
            // object.id = id;
            auto tmp_cells = object.cluster_points;
            // boundingbox_->Compute(object);
            object.heading = lshape2_->Compute(object.cluster_points);
            lshape2_->CoordinateTransformation(object.cluster_points, object.heading);
            boundingbox_->ComputeLWH(object);
            auto tmp_x = object.x;
            auto tmp_y = object.y;
            object.x = tmp_x * std::cos(-object.heading) + tmp_y * std::sin(-object.heading);
            object.y = tmp_y * std::cos(-object.heading) - tmp_x * std::sin(-object.heading);
            // std::cout << " points1 size:" << object.cluster_points.points.size() << std::endl;
            object.cluster_points = tmp_cells;
            // 重新计算角点
            // boundingbox_->ComputeCornerPoints(object);

            // 20以下为车
            GetType(object);
            // id++;
            // if (id > 255)
            //     id = 0;
            // std::cout << " type:" << int(object.type) << " id:" << id << std::endl;

        } // namespace perception
        void Classify::GetType(LidarObject &object)
        {
            int points_size = object.cluster_points.size();
            float z_min = object.z_min;
            float z_max = object.z_max;
            if (object.intensity < 30)
            {
                if (fabs(object.width - 1.6) < 1.0 &&
                    fabs(object.length - 4.5) < 3.5)
                {
                    object.type = static_cast<uint8_t>(ObjectType::CAR);
                    // double det_width = 2.3 - object.width;
                    // double det_length = 4.5 - object.length;
                    // object.width = 2.3;
                    // object.length = 4.5;
                    // object.x = std::cos(object.heading) * det_length + std::sin(object.heading) * det_width + object.x;
                    // object.y = std::cos(object.heading) * det_width + std::sin(object.heading) * det_length + object.y;
                }
                else if (fabs(object.width - 2.5) < 0.8 &&
                         fabs(object.length - 15.0) < 5.0 && object.height > 1.8 &&
                         z_max - z_min > 1.2)
                {
                    object.type = static_cast<uint8_t>(ObjectType::TRUCK);
                }
                else if (fabs(object.width - 0.7) < 0.4 &&
                         fabs(object.length - 1.5) < 0.4 && object.height > 0.8 &&
                         z_max - z_min > 0.5)
                {
                    object.type = static_cast<uint8_t>(ObjectType::CYCLIST);
                }
                else if (fabs(object.width - 0.5) < 0.3 &&
                         fabs(object.length - 0.7) < 0.4 && object.height > 0.8 &&
                         z_max - z_min > 0.5)
                {
                    object.type =
                        static_cast<uint8_t>(ObjectType::PEDESTRIAN);
                }
                else if (fabs(object.width - 0.5) < 0.3 &&
                         fabs(object.length - 0.7) < 0.4 && object.height > 0.8 &&
                         z_max - z_min > 0.5)
                {
                    object.type =
                        static_cast<uint8_t>(ObjectType::PEDESTRIAN);
                }
                else
                    object.type = static_cast<uint8_t>(ObjectType::CAR);
            }
            // 20以上为其他
            else
            {
                // object 3D bounding box is small
                if (fabs(object.width - 1.6) < 1.0 &&
                    fabs(object.length - 4.5) < 3.5)
                {
                    object.type = static_cast<uint8_t>(ObjectType::CAR);
                    // double det_width = 2.3 - object.width;
                    // double det_length = 4.5 - object.length;
                    // object.width = 2.3;
                    // object.length = 4.5;
                    // object.x = std::cos(object.heading) * det_length + std::sin(object.heading) * det_width + object.x;
                    // object.y = std::cos(object.heading) * det_width + std::sin(object.heading) * det_length + object.y;
                }
                else if (fabs(object.width - 2.5) < 0.8 &&
                         fabs(object.length - 15.0) < 5.0 && object.height > 1.8 &&
                         z_max - z_min > 1.2)
                {
                    object.type = static_cast<uint8_t>(ObjectType::TRUCK);
                }
                else if (object.width <= 0.5 && object.length < 0.5 &&
                         fabs(z_max - z_min) < 0.25)
                {
                    // get min height thresh by neighbor points completeness
                    const float z_min_thresh = 0.05;
                    if (fabs(z_max - z_min) < 0.1 &&
                        object.cluster_points.points.size() <= 3) // Lidar Noise Points with dense points
                    {
                        object.type = static_cast<uint8_t>(ObjectType::NOISE);
                    }
                    else if (fabs(z_max - z_min) < 0.1 &&
                             points_size <= 5) // Lidar Noise Points with sparse points
                    {
                        object.type = static_cast<uint8_t>(ObjectType::NOISE);
                    }
                    else
                    {
                        object.type =
                            static_cast<uint8_t>(ObjectType::SHORT_LOW);
                    }
                }
                else if (fabs(object.width - 0.5) < 0.3 &&
                         fabs(object.length - 0.7) < 0.4 && object.height > 0.8 &&
                         z_max - z_min > 0.5)
                {
                    object.type =
                        static_cast<uint8_t>(ObjectType::PEDESTRIAN);
                }
                else
                {
                    object.type = static_cast<uint8_t>(ObjectType::UNKNOWN);
                }
            }
        }

    } // namespace perception
} // namespace avos